package com.xiaov.math;

public class Quaternion {
	
	public float x;
	public float y;
	public float z;
	public float w;
	
	private static final float TOLERANCE = 0.00001f;
	
	public Quaternion(){
		
	}
	
	public Quaternion(float x, float y, float z, float w) {
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
	}
	
	// normalising a quaternion works similar to a vector. This method will not do anything
	// if the quaternion is close enough to being unit-length. define TOLERANCE as something
	// small like 0.00001f to get accurate results
	public void normalise(){
		// Don't normalize if we don't have to
		float mag2 = w * w + x * x + y * y + z * z;
		if (  mag2!=0.f && (Math.abs(mag2 - 1.0f) > TOLERANCE)) {
			float mag = (float)Math.sqrt(mag2);
			w /= mag;
			x /= mag;
			y /= mag;
			z /= mag;
		}
	}
	
	// We need to get the inverse of a quaternion to properly apply a quaternion-rotation to a vector
	// The conjugate of a quaternion is the same as the inverse, as long as the quaternion is unit-length
	public Quaternion getConjugate(){
		return new Quaternion(-x, -y, -z, w);
	}
	
	// Multiplying q1 with q2 applies the rotation q2 to q1
	public Quaternion multiply (Quaternion rq){
		// the constructor takes its arguments as (x, y, z, w)
		
		return new Quaternion(	w * rq.x + x * rq.w + y * rq.z - z * rq.y,
		                  		w * rq.y + y * rq.w + z * rq.x - x * rq.z,
		                  		w * rq.z + z * rq.w + x * rq.y - y * rq.x,
		                  		w * rq.w - x * rq.x - y * rq.y - z * rq.z);
	}

	// Multiplying a quaternion q with a vector v applies the q-rotation to v
	public Vector3D multiply (Vector3D vec){
		Vector3D vn = new Vector3D(vec);
		vn.normalise();

		Quaternion vecQuat = new Quaternion();
		
		vecQuat.x = vn.getX();
		vecQuat.y = vn.getY();
		vecQuat.z = vn.getZ();
		vecQuat.w = 0.0f;

		Quaternion resQuat = vecQuat.multiply(getConjugate());
		resQuat = multiply(resQuat);

		return new Vector3D(resQuat.x, resQuat.y, resQuat.z);
	}
	
	// Convert from Axis Angle
	public void fromAxis(Vector3D v, float angle){
		float sinAngle;
		angle *= 0.5f;
		Vector3D vn = new Vector3D(v);
		vn.normalise();

		sinAngle = (float)Math.sin(angle);

		x = (vn.getX() * sinAngle);
		y = (vn.getY() * sinAngle);
		z = (vn.getZ() * sinAngle);
		w = (float)Math.cos(angle);
	}

	// Convert from Euler Angles
	public void fromEuler(float pitch, float yaw, float roll){
		// Basically we create 3 Quaternions, one for pitch, one for yaw, one for roll
		// and multiply those together.
		// the calculation below does the same, just shorter

		
//		float p = pitch * PIOVER180 / 2.0;
//		float y = yaw * PIOVER180 / 2.0;
//		float r = roll * PIOVER180 / 2.0;
		
//		float p = pitch * MathTools.PI_DIV_180 / 2.0f;
//		float y = yaw * MathTools.PI_DIV_180 / 2.0f;
//		float r = roll * MathTools.PI_DIV_180 / 2.0f;

		float p = pitch / 2.0f;
		float y = yaw / 2.0f;
		float r = roll / 2.0f;
		
		float sinp = (float)Math.sin(p);
		float siny = (float)Math.sin(y);
		float sinr = (float)Math.sin(r);
		float cosp = (float)Math.cos(p);
		float cosy = (float)Math.cos(y);
		float cosr = (float)Math.cos(r);

		x = sinr * cosp * cosy - cosr * sinp * siny;
		y = cosr * sinp * cosy + sinr * cosp * siny;
		z = cosr * cosp * siny - sinr * sinp * cosy;
		w = cosr * cosp * cosy + sinr * sinp * siny;

		normalise();
	}
	
	// Convert to Matrix
	public float[] getMatrix(){
		float x2 = x * x;
		float y2 = y * y;
		float z2 = z * z;
		float xy = x * y;
		float xz = x * z;
		float yz = y * z;
		float wx = w * x;
		float wy = w * y;
		float wz = w * z;

		// This calculation would be a lot more complicated for non-unit length quaternions
		// Note: The constructor of Matrix4 expects the Matrix in column-major format like expected by
		//   OpenGL
		return new float[]{ 1.0f - 2.0f * (y2 + z2), 2.0f * (xy - wz), 2.0f * (xz + wy), 0.0f ,
											 2.0f * (xy + wz), 1.0f - 2.0f * (x2 + z2), 2.0f * (yz - wx), 0.0f ,
											 2.0f * (xz - wy), 2.0f * (yz + wx), 1.0f - 2.0f * (x2 + y2), 0.0f ,
											 0.0f, 0.0f, 0.0f, 1.0f };
	}
	
	// Convert to Axis/Angles
	public float getAxisAngle(Vector3D axis){
		float scale =  (float)Math.sqrt(x * x + y * y + z * z);
		//axis->x = x / scale;
		axis.setX(x / scale);
		//axis->y = y / scale;
		axis.setY(y / scale);
		//axis->z = z / scale;
		axis.setZ(z / scale);
		//*angle = acos(w) * 2.0f;
		return (float)Math.acos(w) * 2.0f;
	}
	
	public Quaternion Quaternion3DMakeWithNLERP(Quaternion start, Quaternion finish, float progress){
		Quaternion ret = new Quaternion();
		float inverseProgress = 1.0f - progress;
		ret.x = (start.x * inverseProgress) + (finish.x * progress);    
		ret.y = (start.y * inverseProgress) + (finish.y * progress);
		ret.z = (start.z * inverseProgress) + (finish.z * progress);
		ret.w = (start.w * inverseProgress) + (finish.w * progress);
	    ret.normalise();
	    return ret;
	}
	
	public Quaternion Quaternion3DMakeWithSLERP(Quaternion start, Quaternion finish, float progress){
		float startWeight, finishWeight, difference;
		Quaternion ret = new Quaternion();;
	  
		difference = ((start.x * finish.x) + (start.y * finish.y) + (start.z * finish.z) + (start.w * finish.w));
		if ((1.f - Math.abs(difference)) > .01f){
	       float theta, oneOverSinTheta;
	      
	       theta = (float)Math.acos(Math.abs(difference));
	       oneOverSinTheta = (float)(1.f / Math.sin(theta));
	       startWeight = (float)(Math.sin(theta * (1.f - progress)) * oneOverSinTheta);
	       finishWeight = (float)(Math.sin(theta * progress) * oneOverSinTheta);
	       if (difference < 0.f)
	           startWeight = -startWeight;
		} 
		else{
	       startWeight = (1.f - progress);
	       finishWeight = progress;
		}
		ret.x = (start.x * startWeight) + (finish.x * finishWeight);
		ret.y = (start.y * startWeight) + (finish.y * finishWeight);
		ret.z = (start.z * startWeight) + (finish.z * finishWeight);
		ret.w = (start.w * startWeight) + (finish.w * finishWeight);
		ret.normalise();
		return ret;
	}
	
	public String toString(){
		return "x: "+ x + "  y: " + y + "  z: " + z + "  w: " + w;
	}
	
}
